#include <string.h>
#include <math.h>

#include "mma7361.h"
#include "eeprom.h"
#include "adc.h"
#include "tick.h"
#include "crc7.h"


#undef TRACE_LEVEL
#undef TRACE_TITLE
#undef TRACE_TITLE
#define TRACE_LEVEL	TRACE_LEVEL_DEBUG
#define TRACE_NAME  TRACE_NAME_DISABLE
#define TRACE_TITLE "MMA"
#include "trace.h"


#define DEF_READ_TIMES			4096
#define DEF_READ_TIMES_SHIFT	12

#define PIPE_DEEP			4
#define PIPE_DEEP_SHIFT		2

#define EEPROM_PAGE_SIZEB	16

#define X_AXIS_1G5_EEBASE	16
#define Y_AXIS_1G5_EEBASE	(X_AXIS_1G5_EEBASE + EEPROM_PAGE_SIZEB)
#define Z_AXIS_1G5_EEBASE	(Y_AXIS_1G5_EEBASE + EEPROM_PAGE_SIZEB)
#define X_AXIS_6G_EEBASE	(Z_AXIS_1G5_EEBASE + EEPROM_PAGE_SIZEB)
#define Y_AXIS_6G_EEBASE	(X_AXIS_6G_EEBASE + EEPROM_PAGE_SIZEB)
#define Z_AXIS_6G_EEBASE	(Y_AXIS_6G_EEBASE + EEPROM_PAGE_SIZEB)

#define X_AXIS_1G5_POS1G_DEF	2930
#define X_AXIS_1G5_NEG1G_DEF	1036

#define Y_AXIS_1G5_POS1G_DEF	3059
#define Y_AXIS_1G5_NEG1G_DEF	1121

#define Z_AXIS_1G5_POS1G_DEF	2730
#define Z_AXIS_1G5_NEG1G_DEF	798

#define X_AXIS_6G_POS1G_DEF		2930
#define X_AXIS_6G_NEG1G_DEF		1036

#define Y_AXIS_6G_POS1G_DEF		3059
#define Y_AXIS_6G_NEG1G_DEF		1121

#define Z_AXIS_6G_POS1G_DEF		2730
#define Z_AXIS_6G_NEG1G_DEF		798

typedef struct {
	uint16_t POS_1G;
	uint16_t NEG_1G;
	uint16_t ZERO_G;
	double DivG;
} Axis_Correction_t;

typedef enum {
	GF_RANGE_1G5,
	GF_RANGE_6G
} Gf_Range_t;


static Gf_Range_t MMA_RANGE = GF_RANGE_1G5;

static Axis_Correction_t X_1G5;
static Axis_Correction_t Y_1G5;
static Axis_Correction_t Z_1G5;
static Axis_Correction_t X_6G;
static Axis_Correction_t Y_6G;
static Axis_Correction_t Z_6G;

static void MMA_IO_Config(void);
static double ADCValueToMilliVolt(uint16_t ADC_Value);
static uint16_t insertPipe(uint16_t *buf, int pipeSizeHW, uint16_t data);
static void readAverageXYZ(uint16_t *x, uint16_t *y, uint16_t *z, int times, int shift);
static double ADCValueToAngle(uint16_t _0G_Value, double divG, uint16_t ADC_Value);
static double ADCValueToMilliVolt(uint16_t ADC_Value);
static double ADCValueToGForce(uint16_t ADC_Value, double divG);
static void MMA_Load_Correct_EE(void); 
static int inputAxisCData(Axis_Correction_t *dat, uint16_t POS1g, uint16_t HEG1g);

// static int PipeInit = 0;
static int MMA_AVG_TIMES = DEF_READ_TIMES;
static int MMA_AVG_TIMES_SHIFT = DEF_READ_TIMES_SHIFT;
static int MMA_PIPE_ENABLE = 1;

static uint16_t xPipe[PIPE_DEEP];
static uint16_t yPipe[PIPE_DEEP];
static uint16_t zPipe[PIPE_DEEP];


/*
** @ Open MMA7361 in SLEEP mode and +-1.5G range
**/
int MMA_Open() {
	/* Open ADC for X,Y,Z axis value read */
	ADC_Open(ADC1);
	
	/* Config the IOs */
	MMA_IO_Config();
	
	MMA_Sleep(ENABLE);
	MMA_Select_1G5();
	
	/* Correct axis data */
	MMA_Load_Correct_EE();
	
	/* filter for Angle calculate */
	//MMA_AVG_TIMES = 0;
	//MMA_AVG_TIMES_SHIFT = 0;
	//MMA_PIPE_ENABLE = 0;
	
	memset(xPipe, 0, sizeof(xPipe));
	memset(yPipe, 0, sizeof(yPipe));
	memset(zPipe, 0, sizeof(zPipe));
	return 0;
}

/*
** @ Close MMA7361
**/
int MMA_Close() {
	ADC_Close(ADC1);
	
	MMA_Select_6G();
	
	MMA_Sleep(ENABLE);
	return 0;
}

/*
** @ Config MMA7361 read mode
**/
int MMA_Read_Mode_Config(int avrTimes, int avgShift, int PipeEnable) {
	int i;
	uint32_t temp = 1;
	
	/* check the shift value */
	for(i = 0; i < avgShift; i ++) {
		temp *= 2;
	}
	
	if (temp != avrTimes)
		return -1;
	
	MMA_AVG_TIMES = avrTimes;
	MMA_AVG_TIMES_SHIFT = avgShift;
	MMA_PIPE_ENABLE = PipeEnable;
	return 0;
}

/*
** @ Read
**/
void MMA_Read(uint16 *x, uint16_t *y, uint16_t *z) {
	readAverageXYZ(x, y, z, MMA_AVG_TIMES, MMA_AVG_TIMES_SHIFT);
	
	if (MMA_PIPE_ENABLE) {
		*x = insertPipe(xPipe, PIPE_DEEP, *x);
		*y = insertPipe(yPipe, PIPE_DEEP, *y);
		*z = insertPipe(zPipe, PIPE_DEEP, *z);
	}
}	

/*
** @ Read G-Force
**/
void MMA_Read_GForce(double *gfx, double *gfy, double *gfz) {
	uint16_t x, y, z;
	
// 	readAverageXYZ(&x, &y, &z, times, shift);
	MMA_Read(&x, &y, &z);
	
	switch(MMA_RANGE) {
	case GF_RANGE_1G5:
		*gfx = ADCValueToGForce(x, X_1G5.DivG);
		*gfy = ADCValueToGForce(y, Y_1G5.DivG);
		*gfz = ADCValueToGForce(z, Z_1G5.DivG);
	break;
	case GF_RANGE_6G:
		*gfx = ADCValueToGForce(x, X_6G.DivG);
		*gfy = ADCValueToGForce(y, Y_6G.DivG);
		*gfz = ADCValueToGForce(z, Z_6G.DivG);
	break;		
	}	
}

/*
** @ Read milliVolt
**/
void MMA_Read_Millivolt(double *mvx, double *mvy, double *mvz) {
	uint16_t x, y, z;
	
	MMA_Read(&x, &y, &z);
	
	*mvx = ADCValueToMilliVolt(x);
	*mvy = ADCValueToMilliVolt(y);
	*mvz = ADCValueToMilliVolt(z);
}

/*
** @ Read Angle
**/
void MMA_Read_Angle(double *anglex, double *angley, double *anglez) {
	uint16_t xbuf, ybuf, zbuf;

	MMA_Read(&xbuf, &ybuf, &zbuf);

	switch(MMA_RANGE) {
	case GF_RANGE_1G5:
		*anglex = ADCValueToAngle(X_1G5.ZERO_G, X_1G5.DivG, xbuf);
		*angley = ADCValueToAngle(Y_1G5.ZERO_G, Y_1G5.DivG, ybuf);
		*anglez = ADCValueToAngle(Z_1G5.ZERO_G, Z_1G5.DivG, zbuf); 
	break;
	case GF_RANGE_6G:
		*anglex = ADCValueToAngle(X_6G.ZERO_G, X_6G.DivG, xbuf);
		*angley = ADCValueToAngle(X_6G.ZERO_G, Y_6G.DivG, ybuf);
		*anglez = ADCValueToAngle(X_6G.ZERO_G, Z_6G.DivG, zbuf); 
	break;		
	}		
}

/*
** @ ENABLE or DISABLE Sleep mode
**/
void MMA_Sleep(FunctionalState state) {
	if(state == ENABLE) {
		GPIO_ResetBits(MMA_CTRL_GPIO, MMA_SLEEP_PIN);
	} else {
		GPIO_SetBits(MMA_CTRL_GPIO, MMA_SLEEP_PIN);
	}
}

/*
** @ Select MMA7361 6GForce range
**/
void MMA_Select_6G(void) {
	GPIO_SetBits(MMA_CTRL_GPIO, MMA_GSEL_PIN);
	MMA_RANGE = GF_RANGE_6G;
}

/*
** @ Select MMA7361 1G5Force range
**/
void MMA_Select_1G5(void) {
	GPIO_ResetBits(MMA_CTRL_GPIO, MMA_GSEL_PIN);
	MMA_RANGE = GF_RANGE_1G5;
}

/* Get Axis Correction Data EE Base Address */
static int Get_EE_Base(char Axis, int Range) {
	int address;
	
	switch(Axis) {
	case 'X':
	case 'x':
		switch(Range) {
		case 1500: address = X_AXIS_1G5_EEBASE;	break;
		case 6000: address = X_AXIS_6G_EEBASE;	break;
		default: address = -1;
		}			
	break;
	case 'Y':
	case 'y':
		switch(Range) {
		case 1500: address = Y_AXIS_1G5_EEBASE; break;
		case 6000: address = Y_AXIS_6G_EEBASE;	break;
		default: address = -1;
		}			
	break;
	case 'Z':
	case 'z':
		switch(Range) {
		case 1500: address = Z_AXIS_1G5_EEBASE;	break;
		case 6000: address = Z_AXIS_6G_EEBASE;	break;
		default: address = -1;
		}			
	break;
	default:
		address = -1;
	}
	
	return address;
}

/* Save Axis Correction Data to EEPROM */
static int MMA_Save_Axis_CorrectEE(char Axis, Axis_Correction_t *dat, int Range) {
	uint16_t buffer[3];
	int address;
	int error = 0;
	
	buffer[0] = dat->POS_1G;
	buffer[1] = dat->NEG_1G;
	buffer[2] = crc7(0, (const uint8_t *)buffer, 4);
	
	/* Get EEPROM Base Address */
	address = Get_EE_Base(Axis, Range);
	if (address == -1)
		return -1;
	
	E2PROM_open(1, 0xA0);
	/* Write Enable */
	E2PROM_wp_disable();
	
	if (E2PROM_write(address, (const uint8_t *)buffer, sizeof(buffer)) != 0) {
		error = -1;
	}		
	E2PROM_wp_enable();
	E2PROM_close();	
	return error;
}

/* Load Axis Correction Data to EEPROM */
static int MMA_Load_Axis_CorrectEE(char Axis, Axis_Correction_t *dat, int Range) {
	uint16_t buffer[3];
	int address;
	int error = 0;
	
	/* Get EEPROM Base Address */
	address = Get_EE_Base(Axis, Range);
	if (address == -1)
		return -1;
	
	E2PROM_open(1, 0xA0);
	/* Write Enable */
	E2PROM_wp_disable();
	
	if (E2PROM_read(address, (uint8_t *)buffer, sizeof(buffer)) != 0) {
		error = -1;
	}		
	E2PROM_wp_enable();
	E2PROM_close();
	
	if (error != 0)
		return -1;
	
	if (buffer[2] != crc7(0, (const uint8_t *)buffer, 4)) {
		return -1;
	}
	
	return inputAxisCData(dat, buffer[0], buffer[1]);
}

/*
** @ Change Axis Correction Data
**/
int MMA_Change_Axis_Correct(char Axis, uint16_t POS_1G, uint16_t NEG_1G, int Range) {
	Axis_Correction_t *ptr;
	int error = 0;
	
	if (POS_1G < NEG_1G) {
		return -1;
	}
	
	switch(Axis) {
	case 'X':
	case 'x':
		switch(Range) {
		case 1500: ptr = &X_1G5;break;
		case 6000: ptr = &X_6G;	break;
		default: error = -1;
		}			
	break;
	case 'Y':
	case 'y':
		switch(Range) {
		case 1500: ptr = &Y_1G5;break;
		case 6000: ptr = &Y_6G;	break;
		default: error = -1;
		}		
	break;
	case 'Z':
	case 'z':
		switch(Range) {
		case 1500: ptr = &Z_1G5;break;
		case 6000: ptr = &Z_6G;	break;
		default: error = -1;
		}				
	break;
	default:
		error = -1;
	}
	
	if (error == -1) {
		return -1;
	}
	
	inputAxisCData(ptr, POS_1G, NEG_1G);
	return MMA_Save_Axis_CorrectEE(Axis, ptr, Range);
}

/* Load MMA Correct data from eeprom */
static void MMA_Load_Correct_EE(void) {	
	if (MMA_Load_Axis_CorrectEE('X', &X_1G5, 1500) != 0) {
		MMA_Change_Axis_Correct('X', X_AXIS_1G5_POS1G_DEF, X_AXIS_1G5_NEG1G_DEF, 1500);
	}
	
	if (MMA_Load_Axis_CorrectEE('Y', &Y_1G5, 1500) != 0) {
		MMA_Change_Axis_Correct('Y', Y_AXIS_1G5_POS1G_DEF, Y_AXIS_1G5_NEG1G_DEF, 1500);
	}
	
	if (MMA_Load_Axis_CorrectEE('Z', &Z_1G5, 1500) != 0) {
		MMA_Change_Axis_Correct('Z', Z_AXIS_1G5_POS1G_DEF, Z_AXIS_1G5_NEG1G_DEF, 1500);
	}
	
	if (MMA_Load_Axis_CorrectEE('X', &X_6G, 6000) != 0) {
		MMA_Change_Axis_Correct('X', X_AXIS_1G5_POS1G_DEF, X_AXIS_1G5_NEG1G_DEF, 6000);
	}

	if (MMA_Load_Axis_CorrectEE('Y', &Y_6G, 6000) != 0) {
		MMA_Change_Axis_Correct('Y', Y_AXIS_1G5_POS1G_DEF, Y_AXIS_1G5_NEG1G_DEF, 6000);
	}

	if (MMA_Load_Axis_CorrectEE('Z', &Z_6G, 6000) != 0) {
		MMA_Change_Axis_Correct('Z', Z_AXIS_1G5_POS1G_DEF, Z_AXIS_1G5_NEG1G_DEF, 6000);
	}
}


/* Calculate the Axis data value */
static int inputAxisCData(Axis_Correction_t *dat, uint16_t POS1g, uint16_t HEG1g) {
	if (dat == NULL)
		return -1;
	
	dat->POS_1G = POS1g;
	dat->NEG_1G = HEG1g;
	
	dat->ZERO_G = (POS1g - HEG1g) / 2 + HEG1g;
	dat->DivG = 2.0 / (double)(POS1g - HEG1g);
	
	return 0;
}

/* Config the MMA7361 Control line */
static void MMA_IO_Config(void) {
	GPIO_InitTypeDef GPIO_InitStructure;
	
	/* SLEEP Line */
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Pin = MMA_SLEEP_PIN;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
	GPIO_Init(MMA_CTRL_GPIO, &GPIO_InitStructure);
	
	/* G-SELECT Line */
	GPIO_InitStructure.GPIO_Pin = MMA_GSEL_PIN;
	GPIO_Init(MMA_CTRL_GPIO, &GPIO_InitStructure);
	
	/* 0G-DETECT Line */
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_InitStructure.GPIO_Pin = MMA_0GDET_PIN;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(MMA_CTRL_GPIO, &GPIO_InitStructure);
}

/* Convert ADC Value to Millivolt */
static double ADCValueToMilliVolt(uint16_t ADC_Value) {
	return 0.805664062 * (double)ADC_Value;
}

/* Convert ADC Value to GForce */
static double ADCValueToGForce(uint16_t ADC_Value, double divG) {
	return divG * (double)ADC_Value;
}

/* Convert ADC Value to Angle */
static double ADCValueToAngle(uint16_t _0G_Value, double divG, uint16_t ADC_Value) {
	double delta;
	
	if (ADC_Value == _0G_Value)
		return 0.0;
	
	if (ADC_Value > _0G_Value) {
		delta = (ADC_Value - _0G_Value);
	} else {
		delta = (_0G_Value - ADC_Value) * -1;
	}	
	delta *= divG; // We get GForce here
	
	if (delta > 1.0)
		delta = 1.0;
	
	if (delta < -1.0)
		delta = -1.0;
	
	return asin(delta) * 57.29577951;
}

/* Read ADC Value for READ_TIMES and get average value */
static void readAverageXYZ(uint16_t *x, uint16_t *y, uint16_t *z, int times, int shift) {
	uint32_t xSum = 0;
	uint32_t ySum = 0;
	uint32_t zSum = 0;
	uint16_t xbuf, ybuf, zbuf;
	int i;
	
	zbuf = 0;
	for(i = 0; i < times; i ++) {
		// ADC_Read(&xbuf, &ybuf, &zbuf);
		// ADC_Read(&xbuf, &ybuf);
		xSum += xbuf;
		ySum += ybuf;
		zSum += zbuf;
	}
	
	*x = xSum >> shift;
	*y = ySum >> shift;
	*z = zSum >> shift;
}

/* Pipe average */
static uint16_t insertPipe(uint16_t *buf, int pipeSizeHW, uint16_t data) {
	int i;
	uint32_t sum = 0;
	
	for(i = 0; i < (pipeSizeHW - 1); i ++) {
		buf[i] = buf[i + 1];
	}
	buf[pipeSizeHW - 1] =  data;
	
	for(i = 0; i < pipeSizeHW; i ++) {
		sum += buf[i];
	}
	return sum >> PIPE_DEEP_SHIFT;
}

// TEST ONLY ****** TEST ONLY ****** TEST ONLY ****** TEST ONLY ****** TEST ONLY ******
#include "tick.h"
#include "usart.h"

int MMA_Main() {
	double angleX, angleY, angleZ;
	
	MMA_Open();
	MMA_Sleep(DISABLE);
	//MMA_Read_Mode_Config(4096, 12, TRUE);
	
	while(1) {
		MMA_Read_Angle(&angleX, &angleY, &angleZ);
		
		fputc(0x0C, NULL);
		 		
		TRACE_DEBUG_WP("AngleX %+.1f\r\n", angleX);
		TRACE_DEBUG_WP("AngleY %+.1f\r\n", angleY);
		TRACE_DEBUG_WP("AngleZ %+.1f\r\n", angleZ);
	}
}
